/* Core file for MiraMEMS 3-Axis Accelerometer's driver.
 *
 * mir3da_core.c - Linux kernel modules for 3-Axis Accelerometer
 *
 * Copyright (C) 2011-2013 MiraMEMS Sensing Technology Co., Ltd.
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
 * may be copied, distributed, and modified under those terms.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 */
#include "mir3da_core.h"
#include "mir3da_cust.h"
#include "../../init-input.h"

#define MIR3DA_REG_ADDR(REG)				((REG)&0xFF)

#define MIR3DA_OFFSET_THRESHOLD			 20
#define PEAK_LVL							800
#define STICK_LSB						   2000
#define AIX_HISTORY_SIZE					20

typedef struct reg_obj_s {

	short			   addr;
	unsigned char	   mask;
	unsigned char	   value;

} reg_obj_t;

struct gsensor_data_fmt_s {

	unsigned char	   msbw;
	unsigned char	   lsbw;
	unsigned char	   endian;						 /* 0: little endian; 1: big endian */
};

struct gsensor_data_obj_s {

#define MIR3DA_DATA_LEN		 6
	reg_obj_t				   data_sect[MIR3DA_DATA_LEN];
	struct gsensor_data_fmt_s   data_fmt;
};

struct gsensor_obj_s {

	char						asic[10];

	reg_obj_t				   chip_id;
	reg_obj_t				   mod_id;
	reg_obj_t				   soft_reset;
	reg_obj_t				   power;

#define MIR3DA_INIT_SECT_LEN	11
#define MIR3DA_OFF_SECT_LEN	 MIR3DA_OFFSET_LEN
#define MIR3DA_ODR_SECT_LEN	 3

	reg_obj_t				   init_sect[MIR3DA_INIT_SECT_LEN];
	reg_obj_t				   offset_sect[MIR3DA_OFF_SECT_LEN];
	reg_obj_t				   odr_sect[MIR3DA_ODR_SECT_LEN];

	struct gsensor_data_obj_s   data;

	int						 (*calibrate)(MIR_HANDLE handle, int z_dir);
	int						 (*auto_calibrate)(MIR_HANDLE handle, int xyz[3]);
	int						 (*int_ops)(MIR_HANDLE handle, mir_int_ops_t *ops);
	int						 (*get_reg_data)(MIR_HANDLE handle, char *buf);
};

struct gsensor_drv_s {

	struct general_op_s		 *method;

	struct gsensor_obj_s		*obj;
};

typedef enum _asic_type {
	ASIC_NONE,
	ASIC_2511,
	ASIC_2512B,
	ASIC_2513A,
	ASIC_2516,
} asic_type;

typedef enum _mems_type {
	MEMS_NONE,
	MEMS_T4,
	MEMS_T9,
	MEMS_TV03,
	MEMS_RTO3,
	MEMS_GT2,
	MEMS_GT3,
} mems_type;

typedef enum _package_type {
	PACKAGE_NONE,
	PACKAGE_2X2_12PIN,
	PACKAGE_3X3_10PIN,
	PACKAGE_3X3_16PIN,
} package_type;

struct  chip_info_s {
	unsigned char	reg_value;
	package_type	 package;
	asic_type		asic;
	mems_type		mems;
};

struct  chip_info_s gsensor_chip_info;

static struct chip_info_s mir3da_chip_info_list[] = {
	{0x00, PACKAGE_2X2_12PIN, ASIC_2512B, MEMS_TV03},
	{0x01, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T4},
	{0x02, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
	{0x03, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T4},
	{0x04, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T9},
	{0x05, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T4},
	{0x06, PACKAGE_3X3_10PIN, ASIC_2511, MEMS_T9},
	{0x07, PACKAGE_3X3_16PIN, ASIC_2511, MEMS_T4},
	{0x08, PACKAGE_3X3_16PIN, ASIC_2511, MEMS_T9},
	{0x09, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T4},
	{0x0c, PACKAGE_2X2_12PIN, ASIC_2512B, MEMS_T9},
	{0x33, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
	{0x34, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
	{0x35, PACKAGE_2X2_12PIN, ASIC_2511, MEMS_T9},
};

#define MIR3DA_NSA_INIT_SECTION						{{ NSA_REG_G_RANGE,			  0xFF,   0x41},								  \
													{ NSA_REG_POWERMODE_BW,		 0xFF,   0x3e},								  \
													{ NSA_REG_ODR_AXIS_DISABLE,	 0xFF,   0x07},								  \
													{ NSA_REG_INTERRUPT_SETTINGS2,  0xFF,   0x00},								  \
													{ NSA_REG_INTERRUPT_MAPPING2,   0xFF,   0x00},								  \
													{ NSA_REG_ENGINEERING_MODE,	 0xFF,   0x83},								  \
													{ NSA_REG_ENGINEERING_MODE,	 0xFF,   0x69},								  \
													{ NSA_REG_ENGINEERING_MODE,	 0xFF,   0xBD},								  \
													{ NSA_REG_INT_PIN_CONFIG,	   0x0F,   0x05},								  \
													{ -1,						   0x00,   0x00},								  \
													{ -1,						   0x00,   0x00}}								  \


#define MIR3DA_NSA_OFFSET_SECTION					{{ NSA_REG_COARSE_OFFSET_TRIM_X, 0xFF,   0x00},								  \
													{ NSA_REG_COARSE_OFFSET_TRIM_Y, 0xFF,   0x00},								  \
													{ NSA_REG_COARSE_OFFSET_TRIM_Z, 0xFF,   0x00},								  \
													{ NSA_REG_FINE_OFFSET_TRIM_X,   0xFF,   0x00},								  \
													{ NSA_REG_FINE_OFFSET_TRIM_Y,   0xFF,   0x00},								  \
													{ NSA_REG_FINE_OFFSET_TRIM_Z,   0xFF,   0x00},								  \
													{ NSA_REG_CUSTOM_OFFSET_X,	  0xFF,   0x00},								  \
													{ NSA_REG_CUSTOM_OFFSET_Y,	  0xFF,   0x00},								  \
													{ NSA_REG_CUSTOM_OFFSET_Z,	  0xFF,   0x00},}								  \

#define MIR3DA_NSA_ODR_SECTION						{{ NSA_REG_ODR_AXIS_DISABLE,	 0x0F,   0x06},								  \
													{ NSA_REG_ODR_AXIS_DISABLE,	 0x0F,   0x07},								  \
													{ NSA_REG_ODR_AXIS_DISABLE,	 0x0F,   0x08}}								  \

#define MIR3DA_NSA_DATA_SECTION					   {{{ NSA_REG_ACC_X_LSB,			0xFF,   0x00},								  \
													{ NSA_REG_ACC_X_MSB,			0xFF,   0x00},								  \
													{ NSA_REG_ACC_Y_LSB,			0xFF,   0x00},								  \
													{ NSA_REG_ACC_Y_MSB,			0xFF,   0x00},								  \
													{ NSA_REG_ACC_Z_LSB,			0xFF,   0x00},								  \
													{ NSA_REG_ACC_Z_MSB,			0xFF,   0x00}},								\
													{ 8,							5,	  0}}

static int NSA_NTO_calibrate(MIR_HANDLE handle, int z_dir);
static int NSA_NTO_auto_calibrate(MIR_HANDLE handle, int xyz[3]);
#if MIR3DA_AUTO_CALIBRATE
static int mir3da_auto_calibrate(MIR_HANDLE handle, int x, int y, int z);
#endif /* !MIR3DA_AUTO_CALIBRATE */
static int NSA_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops);
static int NSA_get_reg_data(MIR_HANDLE handle, char *buf);

#define MIR_NSA_NTO					 {"NSA_NTO",	{NSA_REG_WHO_AM_I, 0xFF, 0x13},								  \
														{NSA_REG_FIFO_CTRL, 0xFF, 0x00},								  \
														{NSA_REG_SPI_I2C, 0x24, 0x24},								  \
														{NSA_REG_POWERMODE_BW, 0x80, 0x80},								  \
														MIR3DA_NSA_INIT_SECTION,								  \
														MIR3DA_NSA_OFFSET_SECTION,								  \
														MIR3DA_NSA_ODR_SECTION,								  \
														MIR3DA_NSA_DATA_SECTION,								  \
														  NSA_NTO_calibrate,								  \
														  NSA_NTO_auto_calibrate,								  \
														  NSA_interrupt_ops,								  \
														  NSA_get_reg_data,								  \
										}
/**************************************************************** COMMON ***************************************************************************/
#define MIR3DA_GSENSOR_SCHEME		   MIR3DA_SUPPORT_CHIP_LIST

#if YZ_CROSS_TALK_ENABLE
static short yzcross;
#endif

static int z_offset;

/* this level can be modified while runtime through system attribute */
int								 Log_level = DEBUG_ERR;
int								gsensor_mod = -1;		/* Initial value */
int								gsensor_type = -1;		/* Initial value */
static struct gsensor_obj_s		 mir3da_gsensor[] = { MIR3DA_GSENSOR_SCHEME };
struct gsensor_drv_s				mir3da_gsensor_drv;

#define MI_DATA(format, ...)			\
	do {\
		if (DEBUG_DATA&Log_level) {\
			printk(MI_TAG format "\n", ## __VA_ARGS__);\
		} \
	} while (0)
#define MI_MSG(format, ...)			 \
	do {\
		if (DEBUG_MSG&Log_level) {\
			printk(MI_TAG format "\n", ## __VA_ARGS__);\
		} \
	} while (0)
#define MI_ERR(format, ...)			 \
	do {\
		if (DEBUG_ERR&Log_level) {\
			printk(MI_TAG format "\n", ## __VA_ARGS__);\
		} \
	} while (0)
#define MI_FUN						  \
	do {\
		if (DEBUG_FUNC&Log_level) {\
			printk(MI_TAG "%s is called, line: %d\n", __FUNCTION__, __LINE__);\
		} \
	} while (0)
#define MI_ASSERT(expr)				 \
	do {\
		if (!(expr)) {\
			printk("Assertion failed! %s,%d,%s,%s\n",\
				__FILE__, __LINE__, __func__, #expr);\
		} \
	} while (0)

/*
#ifndef MTK_ANDROID_M
#define abs(x) ({ long __x = (x); (__x < 0) ? -__x : __x; })
#endif
*/

#if FILTER_AVERAGE_ENHANCE
typedef struct FilterAverageContextTag{
	int sample_l;
	int sample_h;
	int filter_param_l;
	int filter_param_h;
	int filter_threhold;

	int refN_l;
	int refN_h;

} FilterAverageContext;

typedef struct mir3da_core_ctx_s{
	struct mir3da_filter_param_s	filter_param;
	FilterAverageContext			tFac[3];
} mir3da_core_ctx;

static mir3da_core_ctx	   core_ctx;
#endif

#if MIR3DA_SENS_TEMP_SOLUTION
static int bSensZoom;
#endif

#if MIR3DA_OFFSET_TEMP_SOLUTION
static int is_cali;
char bLoad = FILE_CHECKING;
static char readOffsetCnt;
static char readsubfileCnt;
static unsigned char original_offset[9];
static int mir3da_write_offset_to_file(unsigned char *offset);
static int mir3da_read_offset_from_file(unsigned char *offset);
void manual_load_cali_file(MIR_HANDLE handle);
#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */

#if MIR3DA_STK_TEMP_SOLUTION
static short aixHistort[AIX_HISTORY_SIZE * 3] = {0};
static short aixHistoryIndex;
static char bxstk;
static char bystk;
static char bzstk;

static void addAixHistory(short x, short y, short z)
{
	aixHistort[aixHistoryIndex++] = x;
	aixHistort[aixHistoryIndex++] = y;
	aixHistort[aixHistoryIndex++] = z;
	aixHistoryIndex = (aixHistoryIndex) % (AIX_HISTORY_SIZE * 3);
}

static char isXStick(void)
{
	int i = 0, j = 0, temp = 0;
	for (i = 0; i < AIX_HISTORY_SIZE; i++) {
		if ((abs(aixHistort[i*3]) < STICK_LSB) && (aixHistort[i * 3] != 0)) {
			break;
		}
	}

	for (j = 0; j < AIX_HISTORY_SIZE; j++) {
		temp |= aixHistort[j * 3];
	}

	if (0 == temp)
		return 1;

	return i == AIX_HISTORY_SIZE;
}

static char isYStick(void)
{
	int i = 0, j = 0, temp = 0;
	for (i = 0; i < AIX_HISTORY_SIZE; i++) {
		if ((abs(aixHistort[i * 3 + 1]) < STICK_LSB) && (aixHistort[i * 3 + 1] != 0)) {
			break;
		}
	}

	for (j = 0; j < AIX_HISTORY_SIZE; j++) {
		temp |= aixHistort[j * 3 + 1];
	}

	if (0 == temp)
		return 1;

	return i == AIX_HISTORY_SIZE;
}

static char isZStick(void)
{
	int i = 0, j = 0, temp = 0;
	for (i = 0; i < AIX_HISTORY_SIZE; i++) {
		if ((abs(aixHistort[i * 3 + 2]) < STICK_LSB) && (aixHistort[i * 3 + 2] != 0)) {
			break;
		}
	}

	for (j = 0; j < AIX_HISTORY_SIZE; j++) {
		temp |= aixHistort[j * 3 + 2];
	}

	if (0 == temp)
		return 1;

	return i == AIX_HISTORY_SIZE;
}

#endif /* ! MIR3DA_STK_TEMP_SOLUTION */

int squareRoot(int val)
{
	int r = 0;
	int shift;

	if (val < 0) {
		return 0;
	}

	for (shift = 0; shift < 32; shift += 2) {
		int x = 0x40000000l >> shift;
		if (x + r <= val) {
			val -= x + r;
			r = (r >> 1) | x;
		} else {
			r = r >> 1;
		}
	}

	return r;
}

#if FILTER_AVERAGE_ENHANCE
static short filter_average(short preAve, short sample, int paramN, int *refNum)
{
 #if FILTER_AVERAGE_EX
	if (abs(sample-preAve) > PEAK_LVL && *refNum < 3) {
		 MI_DATA("Hit, sample = %d, preAve = %d, refN =%d\n", sample, preAve, *refNum);
		 sample = preAve;
		 (*refNum)++;
	} else {
		 if (*refNum == 3) {
			preAve = sample;
		 }
		 *refNum  = 0;
	}
#endif

	return preAve + (sample - preAve) / paramN;
}

static int filter_average_enhance(FilterAverageContext *fac, short sample)
{
	if (fac == 0) {
		MI_ERR("0 parameter fac");
		return 0;
	}

	if (fac->filter_param_l == fac->filter_param_h) {
		fac->sample_l = fac->sample_h = filter_average(fac->sample_l, sample, fac->filter_param_l, &fac->refN_l);
	} else {
		fac->sample_l = filter_average(fac->sample_l, sample, fac->filter_param_l, &fac->refN_l);
		fac->sample_h = filter_average(fac->sample_h, sample, fac->filter_param_h, &fac->refN_h);
		if (abs(fac->sample_l- fac->sample_h) > fac->filter_threhold) {
			MI_DATA("adjust, fac->sample_l = %d, fac->sample_h = %d\n", fac->sample_l, fac->sample_h);
			fac->sample_h = fac->sample_l;
		}
	 }

	return fac->sample_h;
}
#endif /* ! FILTER_AVERAGE_ENHANCE */

int mir3da_register_read(MIR_HANDLE handle, short addr, unsigned char *data)
{
	int	 res = 0;

	res = mir3da_gsensor_drv.method->smi.read(handle, MIR3DA_REG_ADDR(addr), data);

	return res;
}

int mir3da_register_read_continuously(MIR_HANDLE handle, short addr, unsigned char count, unsigned char *data)
{
	int	 res = 0;

	res = (count == mir3da_gsensor_drv.method->smi.read_block(handle, MIR3DA_REG_ADDR(addr), count, data)) ? 0 : 1;

	return res;
}

int mir3da_register_write(MIR_HANDLE handle, short addr, unsigned char data)
{
	int	 res = 0;

	res = mir3da_gsensor_drv.method->smi.write(handle, MIR3DA_REG_ADDR(addr), data);

	return res;
}

int mir3da_register_mask_write(MIR_HANDLE handle, short addr, unsigned char mask, unsigned char data)
{
	int	 res = 0;
	unsigned char tmp_data;

	res = mir3da_register_read(handle, addr, &tmp_data);
	if (res) {
		return res;
	}

	tmp_data &= ~mask;
	tmp_data |= data & mask;
	res = mir3da_register_write(handle, addr, tmp_data);

	return res;
}

static int mir3da_read_raw_data(MIR_HANDLE handle, short *x, short *y, short *z)
{
	unsigned char	tmp_data[6] = {0};

	if (mir3da_register_read_continuously(handle, mir3da_gsensor_drv.obj[gsensor_mod].data.data_sect[0].addr, 6, tmp_data) != 0) {
		MI_ERR("i2c block read failed\n");
		return -1;
	}

	*x = ((short)(tmp_data[1] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[0]))>> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
	*y = ((short)(tmp_data[3] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[2]))>> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);
	*z = ((short)(tmp_data[5] << mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.msbw | tmp_data[4]))>> (8-mir3da_gsensor_drv.obj[gsensor_mod].data.data_fmt.lsbw);

	MI_DATA("mir3da_raw: x=%d, y=%d, z=%d",  *x, *y, *z);

#if MIR3DA_SENS_TEMP_SOLUTION
	if (bSensZoom == 1) {
		*z = (*z) * 5 / 4;
		MI_DATA("SensZoom take effect, Zoomed Z = %d", *z);
	}
#endif

#if YZ_CROSS_TALK_ENABLE
	if (yzcross)
	  *y = *y - (*z) * yzcross / 100;
#endif

	return 0;
}

static int remap[8][4] = {{0, 0, 0, 0},
					{0, 1, 0, 1},
					{1, 1, 0, 0},
					{1, 0, 0, 1},
					{1, 0, 1, 0},
					{0, 0, 1, 1},
					{0, 1, 1, 0},
					{1, 1, 1, 1}};

int mir3da_direction_remap(short *x, short *y, short *z, int direction)
{
	short temp = 0;

	*x = *x - ((*x) * remap[direction][0] * 2);
	*y = *y - ((*y) * remap[direction][1] * 2);
	*z = *z - ((*z) * remap[direction][2] * 2);

	if (remap[direction][3]) {
		temp = *x;
		*x = *y;
		*y = temp;
	}

	if (remap[direction][2])
		return -1;

	return 1;
}

int mir3da_read_data(MIR_HANDLE handle, short *x, short *y, short *z)
{
	int	rst = 0;

#if MIR3DA_OFFSET_TEMP_SOLUTION
	static int resume_times;
#endif
	int tmp_x = 0, tmp_y = 0, tmp_z = 0;

#if MIR3DA_SUPPORT_MULTI_LAYOUT
	short temp = 0;
#endif

#if MIR3DA_OFFSET_TEMP_SOLUTION
	if (is_cali) {
		*x = *y = *z = 0;
		return 0;
	}

#endif

	rst = mir3da_read_raw_data(handle, x, y, z);

	if (rst != 0) {
		MI_ERR("mir3da_read_raw_data failed, rst = %d", rst);
		return rst;
	}

#if MIR3DA_AUTO_CALIBRATE
#if MIR3DA_SUPPORT_FAST_AUTO_CALI
	if ((mir3da_gsensor_drv.method->support_fast_auto_cali() && (bLoad != FILE_EXIST))
			|| (bLoad == FILE_NO_EXIST))
#else
	if (bLoad == FILE_NO_EXIST)
#endif
	{
	   mir3da_auto_calibrate(handle, *x, *y, *z);
	}
#endif

#if MIR3DA_STK_TEMP_SOLUTION
	addAixHistory(*x, *y, *z);

	bxstk = isXStick();
	bystk = isYStick();
	bzstk = isZStick();

	if ((gsensor_chip_info.mems == MEMS_TV03 || gsensor_chip_info.mems == MEMS_RTO3)
		&& (gsensor_chip_info.reg_value != 0x4B)
		&& (gsensor_chip_info.reg_value != 0x8C)
		&& (gsensor_chip_info.reg_value != 0xCA)) {
	   if ((bxstk + bystk + bzstk) > 0) {
		   if (resume_times < 20) {
			   resume_times++;
			   MI_DATA("IN USE STK & resume!!\n");
			   mir3da_chip_resume(handle);
		   }
	   } else
		   resume_times = 0;
	} else {
		if ((bxstk + bystk+ bzstk) < 2) {
			if (bxstk)
			*x = squareRoot(1024 * 1024 - (*y) * (*y) - (*z) * (*z));
		if (bystk)
			*y = squareRoot(1024 * 1024 - (*x) * (*x) - (*z) * (*z));
		if (bzstk)
			*z = squareRoot(1024 * 1024 - (*x) * (*x) - (*y) * (*y));
		} else {
			// MI_ERR( "CHIP ERR !MORE STK!\n");
			return 0;
		}
	}
#endif


#if FILTER_AVERAGE_ENHANCE
	*x = filter_average_enhance(&core_ctx.tFac[0], *x);
	*y = filter_average_enhance(&core_ctx.tFac[1], *y);
	*z = filter_average_enhance(&core_ctx.tFac[2], *z);
	MI_DATA("mir3da_filt: x=%d, y=%d, z=%d", *x, *y, *z);
#endif


#if MIR3DA_SUPPORT_MULTI_LAYOUT
	if (gsensor_chip_info.package == PACKAGE_2X2_12PIN) {
		*x = *x;
		*y = *z;
		*z = *z;
	} else if (gsensor_chip_info.package == PACKAGE_3X3_10PIN) {
		temp = *x;
		*x = *y;
		*y = temp;
		*z = *z;
	} else if (gsensor_chip_info.package == PACKAGE_3X3_16PIN) {
		temp = -1 * (*x);
		*x = -1 * (*y);
		*y = temp;
		*z = *z;
	}
#endif

	if ((gsensor_chip_info.reg_value == 0x4B)
		|| (gsensor_chip_info.reg_value == 0x8C)
		|| (gsensor_chip_info.reg_value == 0xCA)
		|| (gsensor_chip_info.mems == MEMS_GT2)) {
		*z = 0;
#if MIR3DA_STK_TEMP_SOLUTION
		bzstk = 1;
#endif

	} else {
		tmp_x = *x;
		tmp_y = *y;
		tmp_z = *z;

		mir3da_temp_calibrate(&tmp_x, &tmp_y, &tmp_z);

		*x = tmp_x;
		*y = tmp_y;
		*z = tmp_z;
	}

	MI_DATA("mir3da_read_data: x=%d, y=%d, z=%d", *x, *y, *z);

	return 0;
}

int cycle_read_xyz(MIR_HANDLE handle, int *x, int *y, int *z, int ncycle)
{
	unsigned int j = 0;
	short raw_x, raw_y, raw_z;

	*x = *y = *z = 0;

	for (j = 0; j < ncycle; j++) {
		raw_x = raw_y = raw_z = 0;
		mir3da_read_raw_data (handle, &raw_x, &raw_y, &raw_z);

		(*x) += raw_x;
		(*y) += raw_y;
		(*z) += raw_z;

		mir3da_gsensor_drv.method->msdelay(5);
	}

	(*x) /= ncycle;
	(*y) /= ncycle;
	(*z) /= ncycle;

	return 0;
}

int mir3da_read_offset(MIR_HANDLE handle, unsigned char *offset)
{
	int	 i, res = 0;

	for (i = 0; i < MIR3DA_OFF_SECT_LEN; i++) {
		if (mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr < 0) {
			break;
		}

		res = mir3da_register_read(handle, mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr, &offset[i]);
		if (res != 0) {
			return res;
		}
	}

	return res;
}

int mir3da_write_offset(MIR_HANDLE handle, unsigned char *offset)
{
	int	 i, res = 0;

	for (i = 0; i < MIR3DA_OFF_SECT_LEN; i++) {
		if (mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr < 0) {
			break;
		}

		res = mir3da_register_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].offset_sect[i].addr, offset[i]);
		if (res != 0) {
			return res;
		}
	}

	return res;
}

#if MIR3DA_OFFSET_TEMP_SOLUTION
static int mir3da_write_offset_to_file(unsigned char *offset)
{
	int	 ret = 0;

	if (0 == mir3da_gsensor_drv.method->data_save)
		return 0;

	ret = mir3da_gsensor_drv.method->data_save(offset);

	MI_MSG("====sensor_sync_write, offset = 0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x", offset[0],
			offset[1], offset[2], offset[3], offset[4], offset[5], offset[6], offset[7], offset[8]);

	return ret;
}

static int mir3da_read_offset_from_file(unsigned char *offset)
{
	int	 ret = 0;
	int	 i = 0, sum = 0;

	if (0 == mir3da_gsensor_drv.method->data_get)
		return -1;

	ret = mir3da_gsensor_drv.method->data_get(offset);

	for (i = 0; i < MIR3DA_OFF_SECT_LEN; i++) {
		sum += offset[i];
	}

	if (sum == 0)
	   return -1;

	MI_MSG("====sensor_sync_read, offset = 0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x,0x%x", offset[0],
			offset[1], offset[2], offset[3], offset[4], offset[5], offset[6], offset[7], offset[8]);

	return ret;
}

void manual_load_cali_file(MIR_HANDLE handle)
{
	unsigned char  offset[MIR3DA_OFFSET_LEN] = {0};

	if (bLoad == FILE_CHECKING) {
		readOffsetCnt++;
		if (readOffsetCnt % 8 == 0) {
			readOffsetCnt = 0;
			MI_ERR("====444 manual_load_cali_file(), bLoad = %d, readOffsetCnt=%d.\n", bLoad, readOffsetCnt);

			if (mir3da_gsensor_drv.method->data_check()) {
				readsubfileCnt++;
				if (!mir3da_read_offset_from_file(offset)) {
					MI_ERR("========= FILE EXIST & WRITE OFFSET!");
					mir3da_write_offset(handle, offset);
					bLoad = FILE_EXIST;
				} else if (5 == readsubfileCnt) {
					MI_ERR("========= NO FILE EXIST!");
					bLoad = FILE_NO_EXIST;
				}
			} else {
				MI_ERR("========= FILE CHECKING....");
				bLoad = FILE_CHECKING;
				readsubfileCnt = 0;
			}
		 }
	}
}

static void mir3da_cali_off_to_lsb(int off, int *coarse, int coarse_step, int *fine, int fine_step)
{
	*coarse = off / coarse_step;
	*fine = 100 * (off - (*coarse) * coarse_step) / fine_step;

	MI_MSG("off = %d; delta_coarse = %d; delta_fine = %d", off, *coarse, *fine);
}

#if MIR3DA_AUTO_CALIBRATE
static int NSA_once_calibrate(MIR_HANDLE handle, int coarse_step[3], int fine_step[3], int xyz[3])
{
	int	 coarse[3] = {0};
	int	 coarse_delta[3] = {0};
	int	 fine[3] = {0};
	int	 fine_delta[3] = {0};
	int	 target[3] = {0};
	int	 i;
	unsigned char   offset_data[9] = {0};

	if (mir3da_read_offset(handle, offset_data)) {
		MI_ERR("Get old offset failed !");
		return -1;
	}
	coarse[0] = offset_data[0] & 0x3f;
	coarse[1] = offset_data[1] & 0x3f;
	coarse[2] = offset_data[2] & 0x3f;
	fine[0] = (((int)offset_data[0] << 2) & 0x300) | offset_data[3];
	fine[1] = (((int)offset_data[1] << 2) & 0x300) | offset_data[4];
	fine[2] = (((int)offset_data[2] << 2) & 0x300) | offset_data[5];

	MI_MSG("Old coarse_x = %d; coarse_y = %d; coarse_z = %d; fine_x = %d; fine_y = %d; fine_z = %d;",
			coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);

	/* 0 means auto detect z direction assume z axis is verticle */
	if ((abs(target[0]) + abs(target[1]) + abs(target[2])) == 0) {
		target[2] = (xyz[2] > 0) ? 1024 : (-1024);
	}

	for (i = 0; i < 3; i++) {
		coarse_step[i] *= coarse[i] >= 32 ? (-1) : 1;
		mir3da_cali_off_to_lsb((xyz[i]-target[i]), &coarse_delta[i], coarse_step[i], &fine_delta[i], fine_step[i]);

		coarse[i] += coarse_delta[i];
		fine[i] += fine_delta[i];
		offset_data[i] = coarse[i] | ((fine[i] >> 2) & 0xc0);
		offset_data[i + 3] = fine[i] & 0xFF;
	}

	if (mir3da_write_offset(handle, offset_data)) {
		MI_ERR("Update offset failed !");
		return -1;
	}
	/* Discard unstable data after offset register changed */
	cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
	if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 10)) {
		return -1;
	}
	MI_MSG("---calibrate_Done, x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d",
			xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);

	return mir3da_write_offset_to_file(offset_data);
}
#endif /* !MIR3DA_AUTO_CALIBRATE */

static int NSA_calibrate(MIR_HANDLE handle, int coarse_step[3], int fine_step[3], int fine_max, int target[3])
{
	int			 i = 0, j = 0;
	unsigned char   ncycle = 20;
	unsigned char   nLoop = 20;
	unsigned char   offset_data[9] = {0};
	unsigned char   fine_ok_map = 0;

	int			 xyz[3] = {0};
	int			 coarse[3] = {0};
	int			 coarse_delta[3] = {0};
	int			 fine[3] = {0};
	int			 fine_delta[3] = {0};

	if ((abs(target[0]) + abs(target[1]) + abs(target[2])) != 0 &&
			(abs(target[0]) + abs(target[1]) + abs(target[2])) != 1024) {
		MI_ERR("Invalid argument !");
		return -1;
	}

	/* 0 means auto detect z direction assume z axis is verticle */
	if ((abs(target[0]) + abs(target[1]) + abs(target[2])) == 0) {
	   if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5)) {
			MI_ERR("check z direction failed\n");
			return -1;
	   }
	   target[2] = (xyz[2] > 0) ? 1024 : (-1024);
	}

	MI_MSG("---Start Calibrate, trim target %d, %d, %d---\n", target[0], target[1], target[2]);

	// Stage1: Coarse tune once
	MI_MSG("---Stage1, coarse tune---");
	// change to 16G mode
	if (mir3da_register_mask_write(handle, NSA_REG_G_RANGE, 0x03, 3)) {
		MI_ERR("i2c mask write failed !\n");
		return -1;
	}

	/* reset coarse offset register */
	mir3da_write_offset(handle, offset_data);
	/* Discard unstable data after offset register changed */
	cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);

	if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)) {
		goto EXIT_16G_MOD;
	}

	for (i = 0; i < 3; i++) {
		/* check rule */
		xyz[i] *= 8;

		coarse[i] = ((xyz[i]-target[i]) > 0) ? 0 : 32;

		MI_MSG("xyz[%d] = %d, coarse[%d] = 0x%x", i, xyz[i], i, coarse[i]);

		coarse_step[i] *= coarse[i] >= 32 ? (-1) : 1;
		mir3da_cali_off_to_lsb((xyz[i]-target[i]), &coarse_delta[i], coarse_step[i], &fine_delta[i], fine_step[i]);

		coarse[i] += coarse_delta[i];
		fine[i] += fine_delta[i];
		mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X + i, 0x3f, (unsigned char)coarse[i]);
	}

	/* Discard unstable data after offset register changed */
	cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
	if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5)) {
		return -1;
	}
	for (i = 0; i < 3; i++) {
		fine[i] += (xyz[i] > 0) ? 0 : fine_max;
		mir3da_register_write(handle, NSA_REG_FINE_OFFSET_TRIM_X + i, (unsigned char)(fine[i] & 0xff));
		mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X + i,
				0xc0, (unsigned char)(0xc0 & (fine[i] >> 2)));
	}

EXIT_16G_MOD:
	// change back to 2G mode
	if (mir3da_register_mask_write(handle, NSA_REG_G_RANGE, 0x03, 0)) {
		MI_ERR("i2c mask write failed !\n");
		return -1;
	}
	/* Discard unstable data after offset register changed */
	cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
	if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)) {
		return -1;
	}
	MI_MSG("---Stage1, coarse tune done: x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d",
			xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);

	// Stage2: Fine tune
	MI_MSG("---Stage2, Fine tune---");
	for (i = 0; i < nLoop; i++) {
		if (0x07 == (fine_ok_map & 0x07)) {
			break;
		}
		/* Discard unstable data after offset register changed */
		cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
		MI_MSG("---Stage2, Fine loop %d", i);
		if (cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], ncycle)) {
			return -1;
		}

		for (j = 0; j < 3; j++) {
			MI_MSG("xyz[%d] = %d, caorse[%d] = 0x%x, fine[%d] = 0x%x", j, xyz[j], j, coarse[j], j, fine[j]);
			if (abs(xyz[j]-target[j]) < MIR3DA_OFFSET_THRESHOLD) {
				fine_ok_map |= (1 << j);
				offset_data[j] = coarse[j] | ((fine[j] >> 2) & 0xc0);
				offset_data[j + 3] = fine[j];
				continue;
			}
			mir3da_cali_off_to_lsb((xyz[j]-target[j]), &coarse_delta[j], coarse_step[j], &fine_delta[j], fine_step[j]);

			coarse[j] += coarse_delta[j];
			fine[j] += fine_delta[j];
			mir3da_register_write(handle, NSA_REG_FINE_OFFSET_TRIM_X+j, (unsigned char)(fine[j] & 0xff));
			mir3da_register_mask_write(handle, NSA_REG_COARSE_OFFSET_TRIM_X + j,
					0xFF, (unsigned char)(0xc0 & (fine[j] >> 2)) | coarse[j]);
		}
	}
	MI_MSG("---Stage2, Fine tune done: x = %d, y = %d, z = %d, coarse_x = %d, coarse_y = %d, coarse_z = %d, fine_x = %d, fine_y = %d, fine_z = %d",
			xyz[0], xyz[1], xyz[2], coarse[0], coarse[1], coarse[2], fine[0], fine[1], fine[2]);

	if (0x07 == (fine_ok_map & 0x07)) {
		goto SUCCESS_EXIT;
	}
#if MIR3DA_STK_TEMP_SOLUTION
	if (0x03 == (fine_ok_map & 0x07)) {
		goto SUCCESS_EXIT;
	}
#endif

	MI_MSG("---calibrate Failed !---");
	return -1;

SUCCESS_EXIT:
	MI_MSG("---calibrate OK !---");
	return mir3da_write_offset_to_file(offset_data);
}

static int NSA_NTO_cali_step_calc(MIR_HANDLE handle, int coarse[3], int x100_fine[3], int x100_cust[3])
{
	int				i;
	unsigned int	   total_gain[3] = {0};
	unsigned char	  coarse_gain = 0;
	unsigned char	  fine_gain[3] = {0};
	unsigned int	   const coarse_gain_map[] = {1000, 1125, 1250, 1375, 500, 625, 750, 875};   /* *1000  */
	unsigned char	  const fine_dig_gain_map[] = {1, 2, 4, 8};

	if (mir3da_register_read_continuously(handle, NSA_REG_SENSITIVITY_TRIM_X, 3, fine_gain) != 0) {
		MI_ERR("i2c block read failed\n");
		return -1;
	}

	if (mir3da_register_read(handle, NSA_REG_SENS_COARSE_TRIM, &coarse_gain) != 0) {
		MI_ERR("i2c block read failed\n");
		return -1;
	}

	for (i = 0; i < 3; i++) {
		// *100*1000
		total_gain[i] = ((1000 + (fine_gain[i] & 0x1F) * 1000 / 32) / 15) *
			fine_dig_gain_map[((fine_gain[i] >> 5) & 0x03)] * coarse_gain_map[coarse_gain & 0x07];
		coarse[i] = (int)(total_gain[i] * 500 / 100000);
		x100_fine[i] = (int)(total_gain[i] * 293 / 100000);
		x100_cust[i] = (int)(total_gain[i] * 390 / 100000);
	}
	MI_MSG("coarse_step_x = %d, coarse_step_y = %d, coarse_step_z = %d\n", coarse[0], coarse[1], coarse[2]);
	MI_MSG("fine_step_x = %d, fine_step_y = %d, fine_step_z = %d\n", x100_fine[0], x100_fine[1], x100_fine[2]);
	MI_MSG("custom_step_x = %d, custom_step_y = %d, custom_step_z = %d\n", x100_cust[0], x100_cust[1], x100_cust[2]);

	return 0;
}
#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */

static int NSA_NTO_calibrate(MIR_HANDLE handle, int z_dir)
{
	int	 result = 0;

#if MIR3DA_OFFSET_TEMP_SOLUTION
	int	 coarse_step[3] = {0};
	int	 fine_step[3] = {0};
	int	 custom_step[3] = {0};
	int	 target[3] = {0};

	unsigned char	 swap_plarity_old = 0;

	/* compute step */
	if (NSA_NTO_cali_step_calc(handle, coarse_step, fine_step, custom_step)) {
		MI_ERR("Compute step failed !");
		return -1;
	}
	target[2] = z_dir * 1024;

	// save swap/plarity old setting
	if (mir3da_register_read(handle, NSA_REG_SWAP_POLARITY, &swap_plarity_old)) {
		MI_ERR("Get SWAP/PLARITY setting failed !");
		return -1;
	}

	if ((gsensor_chip_info.asic == ASIC_2512B) ||
			(gsensor_chip_info.asic == ASIC_2513A) ||
			(gsensor_chip_info.asic == ASIC_2516)) {
		coarse_step[2] = 2 * coarse_step[2];
		target[2] = ((swap_plarity_old & (1 << 1)) == 0) ? (-target[2]) : target[2];
		if (mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, 0x0E)) {
			MI_ERR("Set Plarity failed !");
			return -1;
		}
	} else if (gsensor_chip_info.asic == ASIC_2511) {
		target[2] = ((swap_plarity_old & (1 << 1)) != 0) ? (-target[2]) : target[2];
		if (mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY, 0x0F, 0x00)) {
			MI_ERR("Set Plarity failed !");
			return -1;
		}
	}

	result = NSA_calibrate(handle, coarse_step, fine_step, 0x3ff, target);

	// Restore swap/plarity setting
	if (mir3da_register_mask_write(handle, NSA_REG_SWAP_POLARITY,
				0x0F, swap_plarity_old & 0x0F)) {
		MI_ERR("Restore SWAP/PLARITY setting failed !");
		return -1;
	}
#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
	return result;
}

static int NSA_NTO_auto_calibrate(MIR_HANDLE handle, int xyz[3])
{
	int	 result = 0;

#if MIR3DA_AUTO_CALIBRATE
	int	 coarse_step[3];
	int	 fine_step[3];
	int	 custom_step[3] = {0};
	unsigned char	 swap_plarity_old = 0;
	int	 temp = 0;

	/* compute step */
	if (NSA_NTO_cali_step_calc(handle, coarse_step, fine_step, custom_step)) {
		MI_ERR("Compute step failed !");
		return -1;
	}

	// save swap/plarity old setting
	if (mir3da_register_read(handle, NSA_REG_SWAP_POLARITY, &swap_plarity_old)) {
		MI_ERR("Get SWAP/PLARITY setting failed !");
		return -1;
	}

	if (gsensor_chip_info.asic == ASIC_2512B) {
		coarse_step[2] = 2 * coarse_step[2];

		if ((swap_plarity_old & (1 << 0))) {
		   temp = xyz[0];
		   xyz[0] = ((swap_plarity_old & (1 << 2)) == 0) ? (-xyz[1]) : xyz[1];
		   xyz[1] = ((swap_plarity_old & (1 << 3)) == 0) ? (-temp) : temp;
		} else {
		   xyz[0] = ((swap_plarity_old & (1 << 3)) == 0) ? (-xyz[0]) : xyz[0];
		   xyz[1] = ((swap_plarity_old & (1 << 2)) == 0) ? (-xyz[1]) : xyz[1];
		}
				xyz[2] = ((swap_plarity_old & (1 << 1)) == 0) ? (-xyz[2]) : xyz[2];
	} else if (gsensor_chip_info.asic == ASIC_2511) {
		if ((swap_plarity_old & (1 << 0))) {
		   temp = xyz[0];
		   xyz[0] = ((swap_plarity_old & (1 << 2)) != 0) ? (-xyz[1]) : xyz[1];
		   xyz[1] = ((swap_plarity_old & (1 << 3)) != 0) ? (-temp) : temp;
		} else {
		   xyz[0] = ((swap_plarity_old & (1 << 3)) != 0) ? (-xyz[0]) : xyz[0];
		   xyz[1] = ((swap_plarity_old & (1 << 2)) != 0) ? (-xyz[1]) : xyz[1];
		}

		xyz[2] = ((swap_plarity_old & (1 << 1)) != 0) ? (-xyz[2]) : xyz[2];
	}

	result = NSA_once_calibrate(handle, coarse_step, fine_step, xyz);
#endif /* !MIR3DA_AUTO_CALIBRATE */
	return result;
}

int mir3da_calibrate(MIR_HANDLE handle, int z_dir)
{
	int	 res = 0;

#if MIR3DA_OFFSET_TEMP_SOLUTION
	int	 xyz[3] = {0};

	if (is_cali)
		return -1;
	is_cali = 1;

	/* restore original direction if last calibration was done in a wrong direction */
	mir3da_write_offset(handle, original_offset);

	cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 20);

	res = mir3da_gsensor_drv.obj[gsensor_mod].calibrate(handle, z_dir);
	if (res != 0) {
		MI_ERR("Calibrate failed !");
		mir3da_write_offset(handle, original_offset);
	} else {
		bLoad = FILE_EXIST;
	}

	is_cali = 0;
#endif /* !MIR3DA_OFFSET_TEMP_SOLUTION */
	return res;
}

#if MIR3DA_AUTO_CALIBRATE
#define STABLE_CHECK_SAMPLE_NUM	 10
#define STABLE_CHECK_THRESHOLD	  50000
#define AUTO_CALI_THRESHOLD_XY	  200
#define AUTO_CALI_THRESHOLD_Z	   200

static unsigned char	stable_sample_cnt;
static int			  stable_sample_pow_sum[STABLE_CHECK_SAMPLE_NUM] = {0};
static int			  stable_sample_sum[3] = {0};

static int mir3da_auto_cali_condition_confirm(int x, int y, int z, int ave_xyz[3])
{
	int	max = 0, min = 0;
	int	i;
	int	x_ok = 0, y_ok = 0, z_ok = 0;

	stable_sample_pow_sum[stable_sample_cnt] = x*x + y*y + z*z;
	stable_sample_sum[0] += x;
	stable_sample_sum[1] += y;
	stable_sample_sum[2] += z;
	stable_sample_cnt++;

	MI_MSG("---stable_sample_cnt = %d", stable_sample_cnt);

	if (stable_sample_cnt < STABLE_CHECK_SAMPLE_NUM)
		return -1;
	stable_sample_cnt = 0;

	max = stable_sample_pow_sum[0];
	min = stable_sample_pow_sum[0];
	stable_sample_pow_sum[0] = 0;
	for (i = 1; i < STABLE_CHECK_SAMPLE_NUM; i++) {
		if (stable_sample_pow_sum[i] > max)
			max = stable_sample_pow_sum[i];
		if (stable_sample_pow_sum[i] < min)
			min = stable_sample_pow_sum[i];
		stable_sample_pow_sum[i] = 0;
	}
	MI_MSG("---max = %d; min = %d", max, min);

	ave_xyz[0] = stable_sample_sum[0] / STABLE_CHECK_SAMPLE_NUM;
	stable_sample_sum[0] = 0;
	ave_xyz[1] = stable_sample_sum[1] / STABLE_CHECK_SAMPLE_NUM;
	stable_sample_sum[1] = 0;
	ave_xyz[2] = stable_sample_sum[2] / STABLE_CHECK_SAMPLE_NUM;
	stable_sample_sum[2] = 0;

	MI_MSG("ave_x = %d, ave_y = %d, ave_z = %d", ave_xyz[0], ave_xyz[1], ave_xyz[2]);
	x_ok = (abs(ave_xyz[0]) < AUTO_CALI_THRESHOLD_XY) ? 1 : 0;
	y_ok = (abs(ave_xyz[1]) < AUTO_CALI_THRESHOLD_XY) ? 1 : 0;
	z_ok = (abs(abs(ave_xyz[2]) - 1024) < AUTO_CALI_THRESHOLD_Z) ? 1 : 0;

	if ((abs(max-min) > STABLE_CHECK_THRESHOLD) || ((x_ok + y_ok + z_ok) < 2)) {
		return -1;
	}

	return 0;
}

static int mir3da_auto_calibrate(MIR_HANDLE handle, int x, int y, int z)
{
	int	 res = 0;
	int	 xyz[3] = {0};

	if ((gsensor_chip_info.mems == MEMS_RTO3)
		|| (gsensor_chip_info.reg_value == 0x4B)
		|| (gsensor_chip_info.reg_value == 0x8C)
		|| (gsensor_chip_info.reg_value == 0xCA)
		|| (gsensor_chip_info.mems == MEMS_GT2))
		return -1;

	if (is_cali)
		return -1;
	is_cali = 1;

#if MIR3DA_SUPPORT_FAST_AUTO_CALI
	if (mir3da_gsensor_drv.method->support_fast_auto_cali()) {
		cycle_read_xyz(handle, &xyz[0], &xyz[1], &xyz[2], 5);
	} else {
		if (mir3da_auto_cali_condition_confirm(x, y, z, xyz)) {
			res = -1;
			goto EXIT;
		}
	}
#else
	if (mir3da_auto_cali_condition_confirm(x, y, z, xyz)) {
		res = -1;
		goto EXIT;
	}
#endif

	mir3da_write_offset(handle, original_offset);

	res = mir3da_gsensor_drv.obj[gsensor_mod].auto_calibrate(handle, xyz);
	if (res != 0) {
		MI_ERR("Calibrate failed !");
		mir3da_write_offset(handle, original_offset);
	} else
		bLoad = FILE_EXIST;

EXIT:
	is_cali = 0;

	return res;
}
#endif /* !MIR3DA_AUTO_CALIBRATE */

static int NSA_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops)
{
	switch (ops->type) {
	case INTERRUPT_OP_INIT:

		/* latch */
		mir3da_register_mask_write(handle, NSA_REG_INT_LATCH, 0x0f, ops->data.init.latch);
		/* active level & output mode */
		mir3da_register_mask_write(handle, NSA_REG_INT_PIN_CONFIG, 0x0f,
				ops->data.init.level | (ops->data.init.pin_mod << 1) |
				(ops->data.init.level << 2) | (ops->data.init.pin_mod << 3));
		break;

	case INTERRUPT_OP_ENABLE:
		switch (ops->data.int_src) {
		case INTERRUPT_ACTIVITY:
			/* Enable active interrupt */
			mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x07, 0x07);
			break;
		case INTERRUPT_CLICK:
			/* Enable single and double tap detect */
			mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x30, 0x30);
			break;
		}
		break;

	case INTERRUPT_OP_CONFIG:
		switch (ops->data.cfg.int_src) {
		case INTERRUPT_ACTIVITY:
			mir3da_register_write(handle, NSA_REG_ACTIVE_THRESHOLD, ops->data.cfg.int_cfg.act.threshold);
			mir3da_register_mask_write(handle, NSA_REG_ACTIVE_DURATION, 0x03, ops->data.cfg.int_cfg.act.duration);

			/* Int mapping */
			if (ops->data.cfg.pin == INTERRUPT_PIN1) {
				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, (1 << 2), (1 << 2));
			} else if (ops->data.cfg.pin == INTERRUPT_PIN2) {
				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING3, (1 << 2), (1 << 2));
			}
			break;

		case INTERRUPT_CLICK:

			mir3da_register_mask_write(handle, NSA_REG_TAP_THRESHOLD, 0x1f, ops->data.cfg.int_cfg.clk.threshold);
			mir3da_register_mask_write(handle, NSA_REG_TAP_DURATION, (0x03 << 5) | (0x07),
					(ops->data.cfg.int_cfg.clk.quiet_time << 7) |
					(ops->data.cfg.int_cfg.clk.click_time << 6) |
					(ops->data.cfg.int_cfg.clk.window));

			if (ops->data.cfg.pin == INTERRUPT_PIN1) {
				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING1, 0x30, 0x30);
			} else if (ops->data.cfg.pin == INTERRUPT_PIN2) {
				mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_MAPPING3, 0x30, 0x30);
			}
			break;
		}
		break;

	case INTERRUPT_OP_DISABLE:
		switch (ops->data.int_src) {
		case INTERRUPT_ACTIVITY:
			/* Enable active interrupt */
			mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x07, 0x00);
			break;

		case INTERRUPT_CLICK:
			/* Enable single and double tap detect */
			mir3da_register_mask_write(handle, NSA_REG_INTERRUPT_SETTINGS1, 0x30, 0x00);
			break;
		}
		break;

	default:
		MI_ERR("Unsupport operation !");
	}
	return 0;
}

int mir3da_interrupt_ops(MIR_HANDLE handle, mir_int_ops_t *ops)
{
	int res = 0;

	res = mir3da_gsensor_drv.obj[gsensor_mod].int_ops(handle, ops);
	return res;
}

#if FILTER_AVERAGE_ENHANCE
int mir3da_get_filter_param(struct mir3da_filter_param_s *param)
{
	if (param == 0) {
		MI_ERR("Invalid param!");
		return -1;
	}

	param->filter_param_h = core_ctx.tFac[0].filter_param_h;
	param->filter_param_l = core_ctx.tFac[0].filter_param_l;
	param->filter_threhold = core_ctx.tFac[0].filter_threhold;

	MI_MSG("FILTER param is get: filter_param_h = %d, filter_param_l = %d, filter_threhold = %d",
			param->filter_param_h, param->filter_param_l, param->filter_threhold);

	return 0;
}

int mir3da_set_filter_param(struct mir3da_filter_param_s *param)
{

	if (param == 0) {
		MI_ERR("Invalid param!");
		return -1;
	}

	MI_MSG("FILTER param is set: filter_param_h = %d, filter_param_l = %d, filter_threhold = %d",
			param->filter_param_h, param->filter_param_l, param->filter_threhold);

	core_ctx.tFac[1].filter_param_l = core_ctx.tFac[2].filter_param_l = core_ctx.tFac[0].filter_param_l = param->filter_param_l;
	core_ctx.tFac[1].filter_param_h = core_ctx.tFac[2].filter_param_h = core_ctx.tFac[0].filter_param_h = param->filter_param_h;
	core_ctx.tFac[1].filter_threhold = core_ctx.tFac[2].filter_threhold = core_ctx.tFac[0].filter_threhold = param->filter_threhold;

	return 0;
}
#endif //#if FILTER_AVERAGE_ENHANCE

int mir3da_get_enable(MIR_HANDLE handle, char *enable)
{
	int			 res = 0;
	unsigned char   reg_data = 0;

	res = mir3da_register_read(handle, mir3da_gsensor_drv.obj[gsensor_mod].power.addr, &reg_data);
	if (res != 0) {
		return res;
	}

	 *enable = (reg_data & mir3da_gsensor_drv.obj[gsensor_mod].power.mask) ? 0 : 1;

	return res;
}

int mir3da_set_enable(MIR_HANDLE handle, char enable)
{
	int			 res = 0;
	unsigned char   reg_data = 0;

	if (!enable) {
		reg_data = mir3da_gsensor_drv.obj[gsensor_mod].power.value;
	}

	res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].power.addr,
			mir3da_gsensor_drv.obj[gsensor_mod].power.mask, reg_data);

	return res;
}

static int NSA_get_reg_data(MIR_HANDLE handle, char *buf)
{
	int				 count = 0;
	int				 i;
	unsigned char	   val;

	count += mir3da_gsensor_drv.method->mysprintf(buf + count, "---------start---------");
	for (i = 0; i <= 0xd2; i++) {
		if (i % 16 == 0)
			count += mir3da_gsensor_drv.method->mysprintf(buf + count, "\n%02x\t", i);
		mir3da_register_read(handle, i, &val);
		count += mir3da_gsensor_drv.method->mysprintf(buf + count, "%02X ", val);
	}

	count += mir3da_gsensor_drv.method->mysprintf(buf + count, "\n--------end---------\n");
	return count;
}

int mir3da_get_reg_data(MIR_HANDLE handle, char *buf)
{
	return mir3da_gsensor_drv.obj[gsensor_mod].get_reg_data(handle, buf);
}

int mir3da_set_odr(MIR_HANDLE handle, int delay)
{
	int	 res = 0;
	int	 odr = 0;

	if (delay <= 5) {
	   odr = MIR3DA_ODR_200HZ;
	} else if (delay <= 10) {
	   odr = MIR3DA_ODR_100HZ;
	} else {
	   odr = MIR3DA_ODR_50HZ;
	}

	res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].addr,
			mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].mask, mir3da_gsensor_drv.obj[gsensor_mod].odr_sect[odr].value);
	if (res != 0) {
		return res;
	}

	return res;
}

static int mir3da_soft_reset(MIR_HANDLE handle)
{
	int			 res = 0;
	unsigned char   reg_data;

	reg_data = mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.value;
	res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.addr,
			mir3da_gsensor_drv.obj[gsensor_mod].soft_reset.mask, reg_data);
	mir3da_gsensor_drv.method->msdelay(5);

	return res;
}

int mir3da_module_detect(PLAT_HANDLE handle)
{
	int			 i, res = 0;
	unsigned char   cid, mid;
	int			 is_find = -1;

	/* Probe gsensor module */
	for (i = 0; i < sizeof(mir3da_gsensor) / sizeof(mir3da_gsensor[0]); i++) {
		res = mir3da_register_read(handle, mir3da_gsensor[i].chip_id.addr, &cid);
		if (res != 0) {
			return res;
		}

		cid &= mir3da_gsensor[i].chip_id.mask;
		if (mir3da_gsensor[i].chip_id.value == cid) {
			res = mir3da_register_read(handle, mir3da_gsensor[i].mod_id.addr, &mid);
			if (res != 0) {
				return res;
			}

			mid &= mir3da_gsensor[i].mod_id.mask;
			if (mir3da_gsensor[i].mod_id.value == mid) {
				MI_MSG("Found Gsensor MIR3DA !");
				gsensor_mod = i;
				is_find = 0;
				break;
			}
		}
	}

	return is_find;
}

int mir3da_parse_chip_info(PLAT_HANDLE handle)
{
	unsigned char i = 0, tmp = 0;
	unsigned char reg_value = -1, reg_value1 = -1, reg_value2 = -1;
	char res = -1;

	if (-1 == gsensor_mod)
		return res;

	res = mir3da_register_read(handle, NSA_REG_CHIP_INFO, &reg_value);
	if (res != 0) {
		return res;
	}

	gsensor_chip_info.reg_value	= reg_value;

	if (0 == (reg_value >> 6)) {
		return -1;
	}

	if (!(reg_value & 0xc0)) {
		gsensor_chip_info.asic = ASIC_2511;
		gsensor_chip_info.mems = MEMS_T9;
		gsensor_chip_info.package = PACKAGE_NONE;

		for (i = 0; i < sizeof(mir3da_chip_info_list) / sizeof(mir3da_chip_info_list[0]); i++) {
			if (reg_value == mir3da_chip_info_list[i].reg_value) {
				gsensor_chip_info.package = mir3da_chip_info_list[i].package;
				gsensor_chip_info.asic = mir3da_chip_info_list[i].asic;
				gsensor_chip_info.mems = mir3da_chip_info_list[i].mems;
				break;
			}
		}
	} else {
		gsensor_chip_info.asic = ASIC_2512B;
		gsensor_chip_info.mems = MEMS_T9;
		gsensor_chip_info.package = PACKAGE_NONE;

		gsensor_chip_info.package = (package_type)((reg_value & 0xc0) >> 6);

		if ((reg_value & 0x38) >> 3 == 0x01)
			gsensor_chip_info.asic = ASIC_2512B;
		else if ((reg_value & 0x38) >> 3 == 0x02)
			gsensor_chip_info.asic = ASIC_2513A;
		else if ((reg_value & 0x38) >> 3 == 0x03)
			gsensor_chip_info.asic = ASIC_2516;

		res = mir3da_register_read(handle, NSA_REG_CHIP_INFO_SECOND, &reg_value1);
		if (res != 0) {
			return res;
		}

		if (gsensor_chip_info.asic == ASIC_2512B) {
			res = mir3da_register_read(handle, NSA_REG_MEMS_OPTION, &reg_value);
			if (res != 0) {
			   return res;
			}
			tmp = ((reg_value & 0x01) << 2) | ((reg_value1 & 0xc0) >> 6);
		} else {
			tmp = (reg_value1 & 0xe0) >> 5;
		}

		res = mir3da_register_read(handle, NSA_REG_MEMS_OPTION, &reg_value2);
		if (res != 0) {
		   return res;
		}

		if (tmp == 0x00) {
		  if (reg_value2 & 0x80)
			gsensor_chip_info.mems = MEMS_TV03;
		  else
			gsensor_chip_info.mems = MEMS_T9;
		} else if (tmp == 0x01) {
			gsensor_chip_info.mems = MEMS_RTO3;
		} else if (tmp == 0x03) {
			gsensor_chip_info.mems = MEMS_GT2;
			if ((gsensor_chip_info.reg_value != 0x5A) && (gsensor_chip_info.asic == ASIC_2516))
				gsensor_chip_info.mems = MEMS_GT3;
		} else if (tmp == 0x04) {
			gsensor_chip_info.mems = MEMS_GT3;
		}

#if YZ_CROSS_TALK_ENABLE
		if (reg_value1 & 0x10)
			yzcross = -(reg_value1 & 0x0f);
		else
			yzcross = (reg_value1 & 0x0f);
#endif
	}

	return 0;
}


int mir3da_install_general_ops(struct general_op_s *ops)
{
	if (0 == ops) {
		return -1;
	}

	mir3da_gsensor_drv.method = ops;
	return 0;
}

static int mir3da_check_temp_cali_stable_count = 10;

MIR_HANDLE mir3da_core_init(PLAT_HANDLE handle)
{
	int			 res = 0;
#if FILTER_AVERAGE_ENHANCE
	int i = 0;
#endif

	mir3da_gsensor_drv.obj = mir3da_gsensor;

	if (gsensor_mod < 0) {
		res = mir3da_module_detect(handle);
		if (res) {
			MI_ERR("%s: Can't find Mir3da gsensor!!", __func__);
			return 0;
		}

		/* No miramems gsensor instance found */
		if (gsensor_mod < 0) {
			return 0;
		}
	}

	MI_MSG("Probe gsensor module: %s", mir3da_gsensor[gsensor_mod].asic);

#if FILTER_AVERAGE_ENHANCE
	/* configure default filter param */
	for (i = 0; i < 3; i++) {
		core_ctx.tFac[i].filter_param_l = 2;
		core_ctx.tFac[i].filter_param_h = 8;
		core_ctx.tFac[i].filter_threhold = 60;

		core_ctx.tFac[i].refN_l = 0;
		core_ctx.tFac[i].refN_h = 0;
	}
#endif

	res = mir3da_chip_resume(handle);
	if (res) {
		MI_ERR("chip resume fail!!\n");
		return 0;
	}
#if MIR3DA_SUPPORT_FAST_AUTO_CALI
	if ((mir3da_gsensor_drv.method->support_fast_auto_cali != NULL) &&
			(mir3da_gsensor_drv.method->support_fast_auto_cali()))
		mir3da_check_temp_cali_stable_count = 5;
	else
#endif
		mir3da_check_temp_cali_stable_count = 10;

	return handle;
}

int mir3da_chip_resume(MIR_HANDLE handle)
{
	int	 res = 0;
	unsigned char	  reg_data;
	unsigned char	  i = 0;

	res = mir3da_soft_reset(handle);
	if (res) {
		MI_ERR("Do softreset failed !");
		return res;
	}

	for (i = 0; i < MIR3DA_INIT_SECT_LEN; i++) {
		if (mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr < 0) {
			break;
		}

		reg_data = mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].value;
		res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr,
				mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].mask, reg_data);
		if (res != 0) {
			return res;
		}
	}

	mir3da_gsensor_drv.method->msdelay(10);

	if (gsensor_type < 0) {
		gsensor_type = mir3da_parse_chip_info(handle);

		if (gsensor_type < 0) {
			MI_ERR("Can't parse Mir3da gsensor chipinfo!!");
			return -1;
		}
	}

	if (gsensor_chip_info.asic == ASIC_2512B) {

		reg_data = mir3da_gsensor_drv.method->get_address(handle);

		if (reg_data == 0x26 || reg_data == 0x4c) {
			mir3da_register_mask_write(handle, NSA_REG_SENS_COMP, 0xc0, 0x00);
		}
	}


#if MIR3DA_OFFSET_TEMP_SOLUTION
	res = mir3da_read_offset(handle, original_offset);
	if (res != 0) {
		MI_ERR("Read offset failed !");
		return res;
	}

	bLoad = FILE_CHECKING;
	readOffsetCnt = 0;
	readsubfileCnt = 0;

#endif

	return res;
}

int mir3da_get_primary_offset(MIR_HANDLE handle, int *x, int *y, int *z)
{
	int	 res = 0;
	unsigned char	  reg_data;
	unsigned char	  i = 0;
	unsigned char	  offset[9] = {0};

	res = mir3da_read_offset(handle, offset);
	if (res != 0) {
		MI_ERR("Read offset failed !");
		return -1;
	}

	res = mir3da_soft_reset(handle);
	if (res) {
		MI_ERR("Do softreset failed !");
		return -1;
	}

	for (i = 0; i < MIR3DA_INIT_SECT_LEN; i++) {
		if (mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr < 0) {
			break;
		}

		reg_data = mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].value;
		res = mir3da_register_mask_write(handle, mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr,
				mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].mask, reg_data);
		if (res != 0) {
			MI_ERR("Write register[0x%x] error!", mir3da_gsensor_drv.obj[gsensor_mod].init_sect[i].addr);
			goto EXIT;
		}
	}

	mir3da_gsensor_drv.method->msdelay(100);

	res = cycle_read_xyz(handle, x, y, z, 20);
	if (res) {
		MI_ERR("i2c block read failed\n");
		goto EXIT;
	}

	mir3da_write_offset(handle, offset);

	if ((gsensor_chip_info.reg_value == 0x4B)
		|| (gsensor_chip_info.reg_value == 0x8C)
		|| (gsensor_chip_info.reg_value == 0xCA)
		|| (gsensor_chip_info.mems == MEMS_GT2)) {
		*z = 0;
	}

	return 0;

EXIT:
	mir3da_write_offset(handle, offset);
	return -1;
}

#define TEMP_CALIBRATE_STATIC_THRESHOLD	60


int mir3da_temp_calibrate_detect_static(short x, short y, short z)
{
	static int count_static;
	static short pre_x, pre_y, pre_z;
	static int is_first = 1;
	int delta_sum = 0;

	if (is_first == 1) {
		pre_x = x;
		pre_y = y;
		pre_z = z;
		is_first = 0;
	}

	delta_sum = abs(x - pre_x) + abs(y - pre_y) + abs(z - pre_z);

	pre_x = x;
	pre_y = y;
	pre_z = z;

	if (delta_sum < TEMP_CALIBRATE_STATIC_THRESHOLD)
		count_static++;
	else
		count_static = 0;

	MI_MSG("delta_sum=%d count_static=%d", delta_sum, count_static);

	if (count_static >= mir3da_check_temp_cali_stable_count) {
		count_static = mir3da_check_temp_cali_stable_count;
		return 1;
	} else
		return 0;
}

int mir3da_temp_calibrate(int *x, int *y, int *z)
{
	int tem_z = 0;
	int cus = MIR3DA_OFFSET_MAX-MIR3DA_OFFSET_CUS;
	int is_static = 0;
	short lz_offset;

	if (*x == 0 && *y == 0 && *z == 0) {
		MI_MSG("invalid data");
		return -1;
	}

	lz_offset  =  *z % 5;

	MI_MSG("start mir3da_temp_calibrate");

	if ((abs(*x) < MIR3DA_OFFSET_MAX) && (abs(*y) < MIR3DA_OFFSET_MAX)) {
		is_static = mir3da_temp_calibrate_detect_static(*x, *y, *z);
		tem_z = squareRoot(MIR3DA_OFFSET_SEN*MIR3DA_OFFSET_SEN -
				(*x) * (*x) - (*y) * (*y)) + lz_offset;
		if (is_static) {
			if (z_offset) {
				if (abs(abs(*z + z_offset) - MIR3DA_OFFSET_SEN) > MIR3DA_OFFSET_CUS) {
					*z = ((*z >= 0) ? (1) : (-1)) * tem_z;
					z_offset = 0;
				} else {
					*z = (((*z >= 0) ? (1) : (-1)) * tem_z) - z_offset;
				}
			} else {
				z_offset = (*z >= 0) ? (tem_z - *z) : (-tem_z - *z);
			}
		} else if (z_offset == 0) {
			*z = ((*z >= 0) ? (1) : (-1)) * tem_z;
		}

		*x = (*x) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;
		*y = (*y) * MIR3DA_OFFSET_CUS/MIR3DA_OFFSET_MAX;

	} else if ((abs((abs(*x) - MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) &&
			(abs(*y) < MIR3DA_OFFSET_MAX) && (z_offset)) {
		if (abs(*x) > MIR3DA_OFFSET_SEN) {
			*x = (*x > 0) ? (*x - (abs(*x) - MIR3DA_OFFSET_SEN) * cus / MIR3DA_OFFSET_MAX) :
				(*x + (abs(*x) - MIR3DA_OFFSET_SEN) * cus / MIR3DA_OFFSET_MAX);
		} else {
			*x = (*x > 0) ? (*x + (MIR3DA_OFFSET_SEN - abs(*x)) * cus / MIR3DA_OFFSET_MAX) :
				(*x - (MIR3DA_OFFSET_SEN - abs(*x)) * cus / MIR3DA_OFFSET_MAX);
		}
		*y = (*y) * MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX;
	} else if ((abs((abs(*y) - MIR3DA_OFFSET_SEN)) < MIR3DA_OFFSET_MAX) &&
			(abs(*x) < MIR3DA_OFFSET_MAX) && (z_offset)) {
		if (abs(*y) > MIR3DA_OFFSET_SEN) {
			*y = (*y > 0) ? (*y - (abs(*y) - MIR3DA_OFFSET_SEN) * cus / MIR3DA_OFFSET_MAX) :
				(*y + (abs(*y) - MIR3DA_OFFSET_SEN) * cus / MIR3DA_OFFSET_MAX);
		} else {
			*y = (*y > 0) ? (*y + (MIR3DA_OFFSET_SEN - abs(*y)) * cus / MIR3DA_OFFSET_MAX) :
				(*y - (MIR3DA_OFFSET_SEN - abs(*y)) * cus / MIR3DA_OFFSET_MAX);
		}
		*x = (*x)*MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX;
	} else if (z_offset == 0) {
		if ((abs(*x) < MIR3DA_OFFSET_MAX) && (abs((*y > 0) ? (MIR3DA_OFFSET_SEN - *y) :
				(MIR3DA_OFFSET_SEN + *y)) < MIR3DA_OFFSET_MAX)) {
			*z = ((*z >= 0) ? (1) : (-1)) * abs(*x) * MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX;
		} else if ((abs(*y) < MIR3DA_OFFSET_MAX) && (abs((*x > 0) ? (MIR3DA_OFFSET_SEN - *x) :
				(MIR3DA_OFFSET_SEN + *x)) < MIR3DA_OFFSET_MAX)) {
			*z = ((*z >= 0) ? (1) : (-1)) * abs(*y) * MIR3DA_OFFSET_CUS / MIR3DA_OFFSET_MAX;
		} else {
			tem_z = squareRoot(MIR3DA_OFFSET_SEN * MIR3DA_OFFSET_SEN -
					(*x) * (*x) - (*y) * (*y)) + lz_offset;
			*z = ((*z >= 0) ? (1) : (-1)) * tem_z;
		}
	}

	MI_MSG("end mir3da_temp_calibrate z_offset=%d", z_offset);

	if (z_offset) {
#if MIR3DA_OFFSET_TEMP_SOLUTION
		bLoad = FILE_EXIST;
#endif
		*z = *z + z_offset;
		return 0;
	} else
		return -1;
}

